%3.3样条曲线 N个控制点，过起点终点的线
C = [0 1 2 -2 -1 0
     0 0.3 2.5 2.5 4.7 5];  %6个控制点下的曲线
V1 = [1;0];                 %起点C1处的出发方向V1
VN = [1;0];                 %终点CN处的速度方向C2;
L = 0.254/2;

NC = length(C);              %获取控制点数
RC = [C(:,1)-V1*L,C(:,1),C(:,1)+V1*L,C(:,2:5),C(:,6)-VN*L,C(:,6),C(:,6)+VN*L]; %N个控制点后扩充的RC矩阵
N = length(RC);
s = 0:0.01:1;

f1s = (1-s).^3/6;
f2s = (3*s.^3 - 6*s.^2 + 4)/6;
f3s = (-3*s.^3 + 3*s.^2 + 3*s + 1)/6;
f4s = (s.^3)/6;

figure(1);
mycolor = 'mbc';
plot(RC(1,:),RC(2,:),'R*')                                                  %打印出所有控制点（拓展）
hold on;
for i=1:N-3                                                                
    Ps = RC(:,i)*f1s + RC(:,i+1)*f2s + RC(:,i+2)*f3s + RC(:,i+3)*f4s;       %打印出样条曲线轨迹
    plot(Ps(1,:),Ps(2,:),mycolor(mod(i,3)+1));
end
title('过起点和终点的B样条曲线');
grid on;
hold off